// stl相关库
#include <iostream>
#include <vector>
#include <string>
#include <mutex>


// ROS相关库
#include <ros/ros.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>
#include <acupoint_msgs/AcupointImagePixel.h>
#include <acupoint_msgs/AcupointCoordinate.h>

// Eigen相关库
#include<Eigen/Core>
#include<Eigen/Dense>


struct AcuPoint{
    double x;
    double y;
    double z;
    std::string name;
};

std::mutex m;
std::vector<AcuPoint> acupoints;

Eigen::Matrix3d R;
Eigen::Vector3d translation;

// Aruco码Pose回调函数
void arucoPoseCallback(const geometry_msgs::PoseConstPtr &msg){
    std::lock_guard<std::mutex> ld(m);
    Eigen::Quaterniond q;  
    q.x() = msg->orientation.x;  
    q.y() = msg->orientation.y;  
    q.z() = msg->orientation.z;  
    q.w() = msg->orientation.w;  
    Eigen::Matrix3d R_temp;
    translation.x()=msg->position.x;
    translation.y()=msg->position.y;
    translation.z()=msg->position.z;

    // 获得相机相对于Aruco码的齐次变换矩阵
    R_temp = q.normalized().toRotationMatrix();  
    R=R_temp.transpose();
    translation=-R*translation;
    
    std::cout << "R =:\n" <<R<<std::endl;  
    std::cout << "T =\n" << translation << std::endl;
}


// 穴位坐标回调函数
void acupointCallback(const acupoint_msgs::AcupointCoordinateConstPtr &msg){
    std::lock_guard<std::mutex> ld(m);
    if(msg->name.size()!=acupoints.size()){
        acupoints.resize(msg->name.size());
        for(int i=0;i<acupoints.size();++i){
            acupoints[i].name=msg->name[i];
        }
    }
    for(int i=0;i<acupoints.size();++i){
        acupoints[i].x=msg->acupoints[i].x;
        acupoints[i].y=msg->acupoints[i].y;
        acupoints[i].z=msg->acupoints[i].z;
    }
}


int main(int argc, char *argv[])
{
    ros::init(argc, argv, "acu2base_coordinate_node");
    ros::NodeHandle nh;

    // 获得Aruco码相对于机器人底座的齐次变换矩阵
    std::vector<double> temp;
    if(!nh.getParam("aruco_bias",temp)){
        ROS_FATAL_STREAM("aruco_bias Missing");
        return -1;
    }
    Eigen::Vector3d aruco_bias;
    aruco_bias<<temp[0],temp[1],temp[2];
    Eigen::Matrix3d R_base=Eigen::Matrix3d::Identity();
    R_base(0,0)=0;
    R_base(1,0)=1;
    R_base(0,1)=-1;
    R_base(1,1)=0;

    // 定义穴位坐标发布者以及订阅者
    ros::Subscriber aruco_sub=nh.subscribe<geometry_msgs::Pose>("/aruco/aruco_pose",1,arucoPoseCallback);
    ros::Subscriber acu2camera_sub=nh.subscribe<acupoint_msgs::AcupointCoordinate>("/acupoint/acu2camera_coordinate",1,acupointCallback);
    ros::Publisher acu2base_pub=nh.advertise<acupoint_msgs::AcupointCoordinate>("/acupoint/acu2base_coordinate",100);

    ros::Duration(2).sleep();

    ros::Rate rate(1); 


    acupoint_msgs::AcupointCoordinate acu_coordinate_msg;
    bool pub_flag=false;
    while (ros::ok())
    {
        {
            std::lock_guard<std::mutex> ld(m);
            if(acu_coordinate_msg.name.size()!=acupoints.size()){
                acu_coordinate_msg.name.resize(acupoints.size());
                acu_coordinate_msg.acupoints.resize(acupoints.size());
                for(int i=0;i<acu_coordinate_msg.name.size();++i){
                    acu_coordinate_msg.name[i]=acupoints[i].name;
                }
            }
            pub_flag=false;
            Eigen::Vector3d temp_point;
            for(int i=0;i<acupoints.size();++i){
                if(acupoints[i].z>0){
                    pub_flag=true;  //fix：当人不在相机视野时，可能会发布历史穴位坐标数据
                    temp_point.x()=acupoints[i].x;
                    temp_point.y()=acupoints[i].y;
                    temp_point.z()=acupoints[i].z;
                    // 获得穴位相对于机器人底座的坐标
                    Eigen::Vector3d target_point=R_base*R*temp_point+R_base*translation+aruco_bias;
                    acu_coordinate_msg.acupoints[i].x=target_point.x();
                    acu_coordinate_msg.acupoints[i].y=target_point.y();
                    acu_coordinate_msg.acupoints[i].z=target_point.z();
                }
            }

            if(pub_flag){
                // 发布穴位相对于机器人底座的坐标
                acu_coordinate_msg.header.stamp=ros::Time::now();
                acu2base_pub.publish(acu_coordinate_msg);
            }
            
            // 将穴位坐标置为-1
            for(int i=0;i<acupoints.size();++i){
                acupoints[i].z=-100;
            }
        }
        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}




